﻿#include "gait_math.h"
#include "eso.h"
#include "locomotion_header.h"
#include "include.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "optimaize.h"
#if RUN_PI
#include "PositionVelocityEstimator.h"
#endif
GAIT_SHEC gait_etl;
static int gait_init = 0;
void  Gait_ETL_Active(void)
{
	char i = 0;
	GAIT_SHEC gait_zero;
	vmc_all.param.robot_mode = M_ETL;
	vmc_all.gait_mode = G_ETL;
	vmc_all.tar_att[YAWr] = vmc_all.att_ctrl[YAWr];
	vmc_all.tar_pos.z = 0.77*-MAX_Z;// vmc_all.pos_n.z;vmc_all.tar_pos.z = vmc_all.pos_n.z;
	slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
	slip_mode.slip_timer = 0;
#if RUN_PI
	vmc_robot_p.ground_mu = config_gait["walk_param"]["ground_mu"].as<float>();
#endif
	robotwb.exp_att.yaw = robotwb.now_att.yaw;
	for (i = 0; i < 4; i++) {
		vmc[i].tar_sita1 = vmc[i].sita1;
		vmc[i].tar_sita2 = vmc[i].sita2;
		vmc[i].tar_sita3 = vmc[i].sita3;
		vmc[i].ground = 1;
		vmc_all.param.trot_sw_end_flag[i] = 0;
		vmc_all.param.gait_sw_end_reg_n[i] = vmc[i].epos_n;
		vmc[i].st_phase = 0;
		reset_tar_pos(i);
	}
	if (vmc_all.ground_num >= 2 && stand_force_enable_flag[4] && USE_FPOS_CONTROL && !TEST_TROT_SW) {//使能力控
		stand_force_enable_flag[0] = 1;
		stand_force_enable_flag[1] = 1;
		stand_force_enable_flag[2] = 1;
		stand_force_enable_flag[3] = 1;
		stand_force_enable_flag[4] = 1;
		vmc_all.trot_air_test = 0;
		for (i = 0; i < 4; i++) {
			gait_etl.ground_flag[i][1] = gait_etl.ground_flag[i][0] = 1;
		}
	}
	else {
		for (i = 0; i < 4; i++) {
			gait_etl.ground_flag[i][1] = gait_etl.ground_flag[i][0] = 0;
		}
		vmc_all.trot_air_test = 1;
	}
#if RUN_WEBOTS||RUN_PI
	if (vmc_all.trot_air_test)
		printf("ETL Gait Initing::Air Test!!!!!!!\n");
	else
		printf("ETL Gait Initing::Gait Mode!!!!!!!\n");
#endif

	reset_servo_interge();
#if RUN_PI
	kf_pos_vel.rst_kf = 1;
	kf_pos_vel.force_ground = 0;
	moco8_ml();//reset param
#endif


#if RUN_PI
	vmc_all.param.param_vmc_default.stance_time[0] = config_gait["walk_param"]["swing_time"].as<float>();//0.4;//摆动时间
	vmc_robot_p.sw_deltah = config_gait["walk_param"]["swing_height"].as<float>();//0.0525;//摆动高度
	pos_force_p.touch_z_param_sw.st_td = config_gait["walk_param"]["sw_td"].as<float>();//10.0;//N  SW
	pos_force_p.touch_z_param_sw.trot_sw = config_gait["walk_param"]["trot_sw"].as<float>();//10.0;//N  SW
	vmc_all.gait_time[0] = vmc_all.gait_time[1] = vmc_all.param.param_vmc_default.stance_time[0];//???????

	walk_gait.imp_force_time_rate = config_gait["walk_param"]["imp_force_time_rate"].as<float>();//10.0;//N  SW
	walk_gait.imp_alfa[Xr] = config_gait["walk_param"]["imp_alfa_x"].as<float>();//10.0;//N  SW
	walk_gait.imp_alfa[Zr] = config_gait["walk_param"]["imp_alfa_z"].as<float>();//10.0;//N  SW
	gait = gait_zero;
#else

	walk_gait.imp_force_time_rate = 0.3;
	walk_gait.imp_alfa[Xr] = 0.06;
	walk_gait.imp_alfa[Zr] = 50;
#endif
	gait_etl.timer[0] = gait_etl.timer[1] = 0;
	gait_init = 0;
	for (i = 0; i < 4; i++) {
		gait_etl.imp_force[i][Zr] = 0;
		gait_etl.state[i] = 0;
	}


	vmc_all.cog_pos_n.x = vmc_all.cog_pos_n.y = 0;
	vmc_all.zmp_pos_n.x = vmc_all.zmp_pos_n.y = 0;
	vmc_all.cog_spd_n.x = vmc_all.cog_spd_n.y = vmc_all.cog_spd_n.z = 0;
	vmc_all.zmp_spd_n.x = vmc_all.zmp_spd_n.y = vmc_all.zmp_spd_n.z = 0;

	walk_gait.tar_zmp_n.x = walk_gait.tar_zmp_n.y = walk_gait.tar_zmp_n.z = 0;
	walk_gait.tar_zmp_b.x = walk_gait.tar_zmp_b.y = walk_gait.tar_zmp_b.z = 0;
	walk_gait.now_zmp_n.x = walk_gait.now_zmp_n.y = walk_gait.now_zmp_n.z = 0;
	walk_gait.now_zmp_b.x = walk_gait.now_zmp_b.y = walk_gait.now_zmp_b.z = 0;

	walk_gait.trig_id_last = 99;
	walk_gait.trig_id_his[0] = walk_gait.trig_id_his[1] = walk_gait.trig_id_his[2] = walk_gait.trig_id_his[3] = 99;
	walk_gait.xy_control_mode = 1;//锟饺讹拷锟斤拷锟斤拷
	walk_gait.stable_value = 0;
	walk_gait.cog_stable = 0;
	walk_gait.cog_stable_timer = 0;
	walk_gait.min_st_value = 0.005;//use  z 决定质心Stable 
	walk_gait.stable_band = 0.9;//use
	walk_gait.cog_reach_dead = 0.005;//锟斤拷锟侥匡拷锟斤拷锟斤拷锟?	
	walk_gait.work_space_band = 0.95;//锟斤拷锟斤拷锟斤拷锟斤拷呓锟?	
	walk_gait.min_support_area = 0.4;//use
	walk_gait.cog_reach_deadnn = 0.035;//锟斤拷锟侥匡拷锟斤拷锟斤拷锟?全锟斤拷
	walk_gait.sw_h_off_walke = 0.035;//walke模式锟斤拷抬锟斤拷

}


static float vmc_all_gait_alfa = 0.5;
static float S_trot[4] = { 0,0.5,0.5,0 };		 //FR HR FL HL  摆动滞后比例  优先支撑
static float S_stand[4] = { 0,0,0,0 };		 //FR HR FL HL  摆动滞后比例  优先支撑
static float S_bound[4] = { 0.4,0,0.4,0 };		 //FR HR FL HL  摆动滞后比例  优先支撑
static float S_walk[4] = { 0,3,2,1 };		 //FR HR FL HL
//static float S_walk[4] = { 0,3-0.0 -0.3,2 - 0.3,1-0.0 };
static void openloop_phase_gait_schedual(GAIT_SHEC* in, float dt)
{
	char i, j;
	float biggest_S = 0;
	char  id_biggest = in->ref_leg;
	float max_delay[2] = { 0,0 };
	if (!gait_init)
	{
		gait_init = 1;
		in->T_sw = vmc_all.gait_time[0] - vmc_all.stance_time;
		in->T_st = 0.25;
		in->t_gait_change = vmc_all.gait_time[0] * 3;
		in->gait_mode[0] = 0;
		in->w_gait_change = 0.03;
		in->min_st_rate = 0.6;
		in->ref_leg = 0;
		for (i = 0; i < 4; i++) {
			in->ground_flag[i][1] = in->ground_flag[i][0] = 1;
			in->S_now[i] = in->S_target[i] = S_trot[i];
		}
	}
#if 1
	in->T_sw = vmc_all.gait_time[1] * vmc_all_gait_alfa;
	in->T_st = vmc_all.gait_time[1] - in->T_sw;
#else
	in->T_sw = (vmc_all.gait_time[1] - vmc_all.stance_time)*vmc_all_gait_alfa;
	in->T_st = vmc_all.gait_time[1] - vmc_all.stance_time - in->T_sw;
#endif
	in->timer[0] += dt;//system timer 一直递增的计数器

	  //计算该次步态相序所有时间
	for (i = 0; i < 4; i++)
		if (max_delay[0] < in->S_now[i]) {
			max_delay[0] = in->S_now[i];//摆动最大滞后比例
			max_delay[1] = i;
		}

	in->T_all = (in->T_st + in->T_sw)*max_delay[0] + in->T_sw + in->T_st;

	for (i = 0; i < 4; i++) {
		in->sw_start[i] = in->S_now[i] * (in->T_st + in->T_sw);
		in->sw_end[i] = in->sw_start[i] + in->T_st;
	}

	for (i = 0; i < 4; i++)
		in->S_sw[i] = in->S_st[i] = 0;//标志清0
//----------------------------td tirg-----------------------
	if (in->t_ref > 0.99*in->T_all)//参考腿达到步态周期
	{
		in->t_d = in->timer[0];
	}//本次步态参考时间清零

	if (in->timer[1] > in->timer[0])//总时间计数器溢出保护
	{
		in->t_d = in->timer[0];
	}

	in->t_ref = LIMIT(in->timer[0] - in->t_d, -in->T_all, in->T_all);//参考时间以上次结束时间清0
 //---------------------------gait change-----------------------------
	in->gait_mode[0] = vmc_all.gait_mode;
	switch (in->gait_mode[0]) {
		//case TROT://tort
		   // 	for(i=0;i<4;i++)
		   //	 in->S_target[i]=S_tort[i];
		//break;

	}


	for (i = 0; i < 4; i++)
	{
		in->S_now[i] = in->S_target[i] * in->w_gait_change + (1 - in->w_gait_change)*in->S_now[i];
	}
	
	//------------------------------gait planner--------------------------------
	for (i = 0; i < 4; i++) {//以支撑开始 S->0表示立即支撑
		in->ti[i] = LIMIT(in->t_ref, 0, in->T_all);//具有延时的各腿参考时间
		//stance  参考时间大于0则支撑
		if (in->ti[i] > in->sw_start[i] && in->ti[i] <= in->sw_end[i]) {
			in->S_st[i] = 0;
			in->S_sw[i] = 1;
			in->ground_flag[i][0] = 0;
		}
		else
		{
			in->S_st[i] = 1;
			in->S_sw[i] = 0;
			in->ground_flag[i][0] = 1;
		}
	}
	//----------------------------------gait flag-------------------------------
	for (i = 0; i < 4; i++) {
		in->ground_flag[i][1] = in->ground_flag[i][0];
	}
	//----------------------------------reg-------------------------------
	in->gait_mode[1] = in->gait_mode[0];
	in->timer[1] = in->timer[0];
	for (i = 0; i < 4; i++)
	{
		if (in->S_now[i] > biggest_S)
		{
			biggest_S = in->S_now[i];
			id_biggest = i;
		}
	}

	in->S_sw_ref_reg = in->S_sw[(int)max_delay[1]];//最大滞后腿摆动完成比例 作为结束相序参考
}


static float cal_BezierP3_w(float s, float a, float b, float c, float d)//三阶贝塞尔
{
	float temp = 1 - limitw(s, 0, 1);
	float o_temp = a * temp* temp * temp + 3 * b * s * temp * temp + 3 * c * s * s * temp + d * s * s * s;
	return o_temp;
}

static float cal_Impluse_Fz_w(float alfa, float s_st, float s_peak)//Z方力前馈
{
	float fz;
	float s1, s2;

	if (s_st <= s_peak) {
		s1 = s_st / s_peak;
		fz = alfa * cal_BezierP3_w(s1, 0, 0.8, 1, 1);
	}
	else {
		s2 = (s_st - s_peak) / (1 - s_peak);
		fz = alfa * cal_BezierP3_w(s2, 1, 1, 0.8, 0);

	}
	return fz;
}


void setMpcTable_Gait_sel(char leg, char trig_flag)
{

	int id_swap[4] = { 0,2,1,3 };
	int win_id = 0;
	int win_div = 0;//分段位置
	int win_len_next = 0;
	float st_time_all = vmc_all.stance_time;
	float sw_time_all = vmc_all.gait_time[1] - vmc_all.stance_time;
	float sw_time_left;
	float st_time_left;


	if (trig_flag == 1) {//支撑触发
		win_div = LIMIT((st_time_all - robotwb.dtMPC * 0) / robotwb.dtMPC, 0, robotwb._nIterations);

		for (int i = 0; i < robotwb._nIterations; i++) {
			if (i <= win_div)
				robotwb.Leg[leg].mpc_table_s[i] = 1;
			else
				robotwb.Leg[leg].mpc_table_s[i] = 0;
		}

	}
	else//摆动触发
	{
		win_div = LIMIT((sw_time_all - robotwb.dtMPC * 0) / robotwb.dtMPC, 0, robotwb._nIterations);

		for (int i = 0; i < robotwb._nIterations; i++) {
			if (i <= win_div)
				robotwb.Leg[leg].mpc_table_s[i] = 0;
			else
				robotwb.Leg[leg].mpc_table_s[i] = 1;
		}
	}
}


void setMpcTable_Gait_update(char leg, char td_flag, float dt)
{
	static float timer_mpc_update[4] = { 0 };
	int id_swap[4] = { 0,2,1,3 };
	int win_id = 0;
	int win_div = 0;//分段位置
	int win_len_next = 0;
	float st_time_all = vmc_all.stance_time;
	float sw_time_all = vmc_all.gait_time[1] - vmc_all.stance_time;
	float sw_time_left;
	float st_time_left;
	int mpc_table_reg[20];
	timer_mpc_update[leg] += dt;
	if (timer_mpc_update[leg] > robotwb.dtMPC) {
		timer_mpc_update[leg] = 0;

		for (int i = 0; i < robotwb._nIterations; i++) {
			mpc_table_reg[i] = robotwb.Leg[leg].mpc_table_s[i];
		}

		if (td_flag == 1) {//当前支撑中
			win_div = LIMIT((st_time_all - vmc[leg].st_phase) / robotwb.dtMPC, 0, robotwb._nIterations);
			//printf("%d %f %f\n", win_div,st_time_all, vmc[leg].st_phase);
			for (int i = 0; i < robotwb._nIterations; i++) {
				if (i < win_div)
					robotwb.Leg[leg].mpc_table_s[i] = mpc_table_reg[i + 1];
				else
					robotwb.Leg[leg].mpc_table_s[i] = 0;
			}
			robotwb.Leg[leg].mpc_table_s[robotwb._nIterations - 1] = 1;// mpc_table_reg[robotwb._nIterations - 1];
		}
		else//当前摆动中
		{
			win_div = LIMIT((sw_time_all - vmc[leg].sw_phase) / robotwb.dtMPC, 0, robotwb._nIterations);
			for (int i = 0; i < robotwb._nIterations; i++) {
				if (i < win_div)
					robotwb.Leg[leg].mpc_table_s[i] = mpc_table_reg[i + 1];
				else
					robotwb.Leg[leg].mpc_table_s[i] = 1;
			}
			robotwb.Leg[leg].mpc_table_s[robotwb._nIterations - 1] = 0;// mpc_table_reg[robotwb._nIterations - 1];
		}
	}
}

void  Gait_ETL_Update_v1(float dt)
{
	char trig_state_flag[4] = { 0,0,0,0 };
	static char init[4], init_lisence = 0, state, cnt_time_change, switch_flag;
	static float time_delay[4], yaw_force;
	static float timer_task[10];
	char i, sel;
	float test_epos_b[3], test_epos_leg[3];
	static END_POS end_pos_n[4], end_pos_n_use[4];//ĩ�˹̶���ȫ������ϵ�µ�λ��
	static float cnt_mode_can_switch = 0, mode_en_switch_flag = 0;
	static float att_test_use[3];
	char leg_sel_trig = 0;
	static float en_force_timer[3] = { 0 };
	float att_off[2];
	static float phast_time[4] = { 0 };
	static float phast_time_over[4] = { 0 };
#if EN_VISION
#if FMAP_USE_SIMU
	fake_map_link_init();
#else
	fake_map_init(&fake_map_trench_5cm, 0, 0, 0, 0);
#endif
#endif
	if (timer_task[2] > 0.02) {
		body_traj_planner(timer_task[2]);
		timer_task[2] = 0;
	}
#if !USE_MPC||USE_MPC_QP//采用QP计算力仅用于更新步态相序
	GaitLocomotion_run(dt);
#endif
#if EN_VISION
	move_robot_on_map_n(dt);//移动地图
#endif

	if (1) {
		if (vmc_all.gait_mode == G_ETL)//----------------------�Խǲ�̬
		{
			openloop_phase_gait_schedual(&gait_etl, dt);//开环相序
			for (i = 0; i < 4; i++)
			{
				if (vmc[i].ground)
					vmc[i].param.st_time_used += dt;

				switch (gait_etl.state[i])
				{
				case 0:
					if (EN_SW) {

						if (cnt_time_change++ > 4||1) {
							vmc_all.gait_time[1] = vmc_all.gait_time[3];
							vmc_all.stance_time = vmc_all.stance_time_auto;
							vmc_all.delay_time[2] = vmc_all.delay_time[1];
							cnt_time_change = 0;
						}

						phast_time_over[i] += dt;

						//printf("[%d] swingStates=%f contactStates=%f\n", i, robotwb.Leg[i].swingStates, robotwb.Leg[i].contactStates);
						if (//robotwb.Leg[i].mpc_table[1]==0//
							//gait_etl.ground_flag[i][0] ==0
	
							robotwb.Leg[i].swingStates > 0.002&&robotwb.Leg[i].swingStates != 1//MIT相序
							//gait_etl.ground_flag[i][0] == 0//本程序相序
							//&& vmc[i].ground == 1//开环摆动信号
							//&& vmc_all.ground_num>=3
							//&& (walk_gait.cog_stable || !USE_ZMP || phast_time_over[i] > vmc_all.stance_time * 2)
							)//sech to swing
						{

							setMpcTable_Gait_sel(i, 0);
							vmc_all.force_update_mpc = 1;
							//printf("robotwb.Leg[%d].swingStates=%f\n", i,robotwb.Leg[i].swingStates);
							vmc[i].param.trig_state = 1;
							phast_time_over[i] = 0;
#if USE_IMP_PLAN
							gait_etl.state[i] = 1;//to impluse
							gait_etl.imp_force[i][Zr] = 0;
							time_delay[i] = 0;
#else
							trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
							vmc[i].ground = 0;	//清除着地标志位
							stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
							vmc[i].param.trig_state = 1;
							gait_etl.state[i] = 2;//to LF
#endif
						}
					}
					break;
				case 1://Impluse down
#if USE_IMP_PLAN
					time_delay[i] += dt;
					if (i == 0 || i == 2)
						robotwb.exp_pos_n.x = -walk_gait.imp_alfa[Xr] + robotwb.exp_spd_n.x*dt;
					else
						robotwb.exp_pos_n.x = walk_gait.imp_alfa[Xr] + robotwb.exp_spd_n.x*dt;
					gait_etl.imp_force[i][Zr] = cal_Impluse_Fz_w(walk_gait.imp_alfa[Zr], LIMIT(time_delay[i] / (vmc_all.stance_time*walk_gait.imp_force_time_rate), 0, 1), 0.35);
					//printf("i=%d gait_etl.imp_force[i][Zr] =%f\n", i, gait_etl.imp_force[i][Zr]);
					if (time_delay[i] > vmc_all.stance_time*walk_gait.imp_force_time_rate) {
						gait_etl.state[i]++;
						gait_etl.imp_force[i][Zr] = 0;
						time_delay[i] = 0;
						trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
						vmc[i].ground = 0;	//清除着地标志位
						stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
						vmc[i].param.trig_state = 1;
					}
#else
					if (1) {
						gait_etl.state[i]++;
						trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
						vmc[i].ground = 0;	//清除着地标志位
						stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
						vmc[i].param.trig_state = 1;
					}
#endif
					break;
				case 2://LF
					phast_time[i] += dt;
					trig_state_flag[i] = trig_lift(i, dt);

					if (trig_state_flag[i]) {//同时达到？
						vmc[i].param.trig_state = 2;//进入摆动
						gait_etl.state[i]++;
					}

					break;
				case 3://SW&TD
					phast_time[i] += dt;

					switch (vmc[i].param.trig_state)
					{
					case 2:
						trig_state_flag[i] = trig_swing(i, dt);

						if (trig_state_flag[i] == 2) {//摆动着地 之前未添加在舵狗
							vmc[i].param.trig_state = 4;
						}
						else if (trig_state_flag[i] == 1)//摆动结束-->TD
							vmc[i].param.trig_state = 3;
						break;
					case 3:
						trig_state_flag[i] = trig_td(i, dt);
						if (trig_state_flag[i] >= 1) {//TD结束正常
							vmc[i].param.trig_state = 4;
						}
						break;
					}


#if GROUND_AFTER_TRIG||1
					if ((vmc[i].param.trig_state >= 4)
						&& ((robotwb.Leg[i].contactStates > 0.005
							//&& robotwb.Leg[i].contactStates < 0.3
							)
							|| _hu_model.gait_switching
							)
						
#if FIX_ST_TIME
						//&&phast_time[i] > vmc_all.stance_time*MIN_ST_TIME_RATE
#endif
						)//对角腿同时着地
#else
					if ((vmc[[0]].ground&&vmc[id_sw[1]].ground)
#if FIX_ST_TIME
						&&phast_time > vmc_all.stance_time*MIN_ST_TIME_RATE
#endif
						)//对角腿同时着地
#endif
					{
						if (vmc_all.param.stand_trot_switch_flag == 1) {//站立切换Trot
							vmc_all.param.stand_switch_cnt[0]++;
							vmc_all.param.trot_sw_end_flag[i] = 1;
							if (vmc_all.param.stand_switch_cnt[0] >= 1)//切换完毕
							{
								vmc_all.param.stand_switch_cnt[0] = 0;
								vmc_all.param.stand_switch_flag[1] = 0;//支撑内收
								vmc_all.param.stand_trot_switch_flag = 0;
							}
						}

						if (vmc_all.param.stand_trot_switch_flag == 2) {//Tort切换站立
							vmc_all.param.stand_switch_cnt[1]++;
							switch (vmc_all.param.stand_switch_cnt[1])
							{
							case 1:
								vmc_all.param.stand_switch_flag[1] = 1;//支撑外扩
								break;
							case 3://切换完毕
								vmc_all.param.stand_switch_cnt[1] = 0;
								vmc_all.param.stand_trot_switch_flag = 0;
								break;
							}
						}

						setMpcTable_Gait_sel(i, 1);
						vmc_all.force_update_mpc = 1;

						vmc[i].ground = 1;//切换着地符号  非Ground下Load力拉至地面 用于状态估计
						vmc[i].param.trig_state = 0;//复位摆动标志
						stand_force_enable_flag[i] = 1 && !vmc_all.trot_air_test;//启动力控
						vmc[i].param.st_time_used = 0;

						if (cnt_time_change++ > 4||1) {
							vmc_all.gait_time[1] = vmc_all.gait_time[3];
							vmc_all.stance_time = vmc_all.stance_time_auto;
							vmc_all.delay_time[2] = vmc_all.delay_time[1];
							cnt_time_change = 0;
						}

						gait_etl.state[i] = 0;
					}
					break;
				}
			}//end for

			for (i = 0; i < 4; i++) {//输出摆动速度
				if (vmc[i].ground) {
					vmc[i].st_phase += dt;
					vmc[i].sw_phase = 0;
				}
				else {
					vmc[i].st_phase = 0;
					vmc[i].sw_phase += dt;
				}

				setMpcTable_Gait_update(i, vmc[i].ground, dt);
			}
			for (i = 0; i < 4; i++) {//输出摆动速度
				if (vmc[i].param.trig_state >= 1 && vmc[i].param.trig_state <= 3)//
					swing_spd_control(i, dt);

			}
		}
	}	//end of lisence check

	for (i = 0; i < 10; i++)
		timer_task[i] += dt;
}
